function [J] = cost_cons(param, pix, dist)
pos = param(1:2);
J = 0;
n = size(dist, 2);
for i = 1 : n - 1
    for j = i + 1 : n
        if dist(i) > dist(j)
            ma = norm(pos - pix(:, i));
            ra = dist(i);
            mi = norm(pos - pix(:, j));
            ri = dist(j);
        else
            mi = norm(pos - pix(:, i));
            ri = dist(i);
            ma = norm(pos - pix(:, j));
            ra = dist(j);
        end
        J = J + max(0, mi - ma) * (ra - ri);
    end
end

